# Untitled - By: User - 周三 4月 19 2023
import sensor, image,lcd,math,sys
from machine import UART #串口库函数
from fpioa_manager import fm # GPIO重定向函数
sensor_x=320
sensor_y=240
fm.register(18, fm.fpioa.UART1_TX, force=True)
fm.register(19, fm.fpioa.UART1_RX, force=True)
uart_A = UART(UART.UART1, 115200, 8, 0, 1, timeout=1000, read_buf_len=4096)
ce_list=[]
show_ce_list=[]
green_threshold = ((80, 160))           #黑色
roi1            = [0,120,320,16]       #巡线敏感区
roi2            = [0,80 ,320,16]
roi3            = [0,40 ,320,16]
roi4            = [0,240-16,320,16]       #巡线敏感区
def fun_linear_regression(ce_list):
    #https://nihe.91maths.com/linear.php
    # 输入数据
    points = ce_list
    print(points)
    x = [p[0]-sensor_x/2 for p in points]
    y = [sensor_y-p[1]-sensor_y/2 for p in points]
    #print(y)
    # 计算x和y的平均值
    x_mean = sum(x) / len(x)
    y_mean = sum(y) / len(y)

    # 计算斜率
    numerator = sum([(xi - x_mean) * (yi - y_mean) for xi, yi in zip(x, y)])
    denominator = sum([(xi - x_mean) ** 2 for xi in x])
    slope = numerator / denominator
    # 计算截距
    intercept = y_mean - slope * x_mean
    # 输出结果
    distance=x_mean
    print("斜率：", slope)
    #print("截距：", intercept)
    print("中心距离",distance) #<0
    # 计算arctan(m), 结果以弧度表示
    m=slope
    angle = math.atan(m)

    # 将弧度转换为度数
    angle_degree = angle * 180 / math.pi


    angle_degree += 90
    if angle_degree>90:
       angle_degree-=180
    angle_degree=int(-angle_degree+distance/10)
    print("夹角为{angle_degree}度",angle_degree)
    return angle_degree

def draw_string(img, x, y, text, color, scale, bg=None , full_w = False):
    if bg:
        if full_w:
            full_w = img.width()
        else:
            full_w = len(text)*8*scale+4
        img.draw_rectangle(x-2,y-2, full_w, 16*scale, fill=True, color=bg)
    img = img.draw_string(x, y, text, color=color,scale=scale)
    return img
def set_left_cross(img,coordinate_list):
    x_coordinate_list=[]
    for x,y in coordinate_list:
        x_coordinate_list.append(x)
    min_x_idx = x_coordinate_list.index(min(x_coordinate_list))
    min_x, min_y= coordinate_list[min_x_idx]
    #print(min_x, min_y)
    img.draw_cross(min_x, min_y, 0x8811)
    return [min_x,min_y]


def set_right_cross(img,coordinate_list):
    x_coordinate_list=[]
    for x,y in coordinate_list:
        x_coordinate_list.append(x)
    max_x_idx = x_coordinate_list.index(max(x_coordinate_list))
    max_x, max_y= coordinate_list[max_x_idx]
    #print(max_x, max_y)
    img.draw_cross(max_x, max_y, 0x8811)
    return [max_x,max_y]

def sending_data(x,y,z):
    global uart;
    FH = bytearray([0x2C,0x12,x,y,z,0x5B])
    uart_A.write(FH);

lcd.init()
sensor.reset()
sensor.set_pixformat(sensor.GRAYSCALE)
sensor.set_framesize(sensor.QVGA) # 320x240
sensor.skip_frames(time = 3000 )#跳过3000张图片
sensor.set_auto_gain(False) # must be turned off for color tracking
sensor.set_auto_whitebal(False) # must be turned off for color tracking
sensor.set_hmirror(1)
sensor.set_vflip(1)
sensor.run(1)
while True:
    img=sensor.snapshot()
    img.draw_rectangle(roi1)
    img.draw_rectangle(roi2)
    img.draw_rectangle(roi3)
    statistics1 = img.find_blobs([green_threshold],roi=roi1,area_threshold=200,merge=True)
    statistics2 = img.find_blobs([green_threshold],roi=roi2,area_threshold=200,merge=True)
    statistics3 = img.find_blobs([green_threshold],roi=roi3,area_threshold=200,merge=True)
    statistics4 = img.find_blobs([green_threshold],roi=roi4,area_threshold=200,merge=True)
    if statistics1:
        #print(len(statistics1))
        coordinate_list=[]
        for b in statistics1:
            tmp=img.draw_rectangle(b[0:4])
            tmp=img.draw_cross(b[5], b[6])

            c_list=[b[5], b[6]]
            coordinate_list.append(c_list)
        ce=set_left_cross(img,coordinate_list)
        ce_list.append(ce)
        show_ce_list.append('1:')
        show_ce_list.append(ce)
    if statistics2:
        #print(len(statistics2))
        coordinate_list=[]
        for b in statistics2:
            tmp=img.draw_rectangle(b[0:4])
            tmp=img.draw_cross(b[5], b[6])
            c_list=[b[5], b[6]]
            coordinate_list.append(c_list)
        ce=set_left_cross(img,coordinate_list)
        ce_list.append(ce)
        show_ce_list.append('2:')
        show_ce_list.append(ce)
    if statistics3:
        #print(len(statistics3))
        coordinate_list=[]
        for b in statistics3:
            tmp=img.draw_rectangle(b[0:4])
            tmp=img.draw_cross(b[5], b[6])
            c_list=[b[5], b[6]]
            coordinate_list.append(c_list)
        ce=set_left_cross(img,coordinate_list)
        ce_list.append(ce)
        show_ce_list.append('3:')
        show_ce_list.append(ce)
    #print(ce_list)

    #if statistics4:
        ##print(len(statistics3))
        #coordinate_list=[]
        #for b in statistics4:
            #tmp=img.draw_rectangle(b[0:4])
            #tmp=img.draw_cross(b[5], b[6])

    try:
        ar=fun_linear_regression(ce_list)
    except Exception as e:
        ar=0
        #print("An error occurred:", e)
        #sys.print_exception(e)
    img = draw_string(img, 0, 0, str(show_ce_list)+'ar'+str(ar), color=lcd.WHITE,scale=1.2, bg=lcd.BLACK, full_w=True)
    show_ce_list=[]
    ce_list=[]
    lcd.display(img)
    #print(c_list)
            #PID计算

            #actualValue=b[5]
            #err=actualValue-expectedValue
            #Speed_left = Speed - (Kp*err+Kd*(err-old_err))
            #Speed_right = Speed + (Kp*err+Kd*(err-old_err))
            #old_err= err
            #print("Speed_left,Speed_right")
            #print(int(Speed_left),int(Speed_right))
    #if statistics2:
        #for b in statistics2:
            #tmp=img.draw_rectangle(b[0:4])
            #tmp=img.draw_cross(b[5], b[6])
            #if b[2] >50:
                #Flag = 1
    #sending_data(int(Speed_left),int(Speed_right),Flag)
